classdef jointMotor <handle
    properties
       Ke = 8.24e-2;
       Ka = 8.22e-2;
       Ra = 2.49;
       La = 6.1e-4;
       Ku = 3;       %电压放大增益
       Kg = 1;       %电流放大增益 
       
       current = 0;   %额定电流
       voltage = 0;     %额定电压
       
       working_mode = 0; %0 for vel mode, 1 for torque
    end
    
    
    methods
        function obj = jointMotor(working_mode,cur,vol,ke,ka,ra,la,ku,kg)
            if(nargin == 1)
                obj.working_mode = working_mode;
                fprintf("initial with defalut value,using ");  
                if(working_mode == 0)
                    fprintf("vel mode\n");
                else
                    fprintf("torque mode\n");
                end  
            elseif(nargin == 9)
                obj.working_mode = working_mode;
                obj.current = cur;
                obj.voltage = vol;
                obj.Ke  =ke;
                obj.Ka = ka;
                obj.Ra = ra;
                obj.La = la;
                obj.Ku = ku;
                obj.Kg = kg;
            elseif(nargin == 0)
                disp("using motor default value");
            else
                ME = MException('need  9 param for all info or none for defalut or one for mode!');
                throw(ME)
            end                      
        end            
        
        function vel_motor_info = vel_simu_loop(obj,sj_obj,simu_period,con_value)
            
            %             bad_rows = find(err_now(:,1)< -pi,obj.n_group);
%             err_now(bad_rows,1) = mod(err_now(bad_rows,1),pi);
%             bad_rows = find(err_now(:,1)> pi,obj.n_group);
%             err_now(bad_rows,1) = mod(err_now(bad_rows,1),-pi);
            bad_rows = find(con_value(:,1)< -10,obj.n_group);
            con_value(bad_rows,1) = -10;
            bad_rows = find(con_value(:,1)> 10,obj.n_group);
            con_value(bad_rows,1) = 10;

            
            %根据pid输出的控制值来输出力矩(第一个),被控者为电枢电压
            vel_motor_info = zeros(sj_obj.n_group,3);%分别输出电机的力矩，当前电压，电流
            vel_motor_info(:,2) = obj.voltage;%注意·记录·的顺序
            vel_motor_info(:,3) = obj.current;   
            obj.voltage = con_value * obj.Ku;
            ue = obj.Ke*sj_obj.state(:,2);
            obj.current = (obj.La/obj.Ra)/(obj.La/obj.Ra + simu_period)*obj.current + (simu_period/obj.Ra)/(obj.La/obj.Ra + simu_period)*(obj.voltage - ue); % 该时刻电流（一阶惯性系统的离散表达式）
            %obj.current = (obj.voltage-obj.Ke*sj_obj.state(2))/obj.Ra;
            vel_motor_info(:,1) = obj.Ka * obj.current;
        end
        
        function torque_motor_info = torque_simu_loop(obj,sj_obj,con_value)
            %根据pid输出的控制值来输出力矩,被控者为电枢电流
            torque_motor_info = zeros(sj_obj.n_group,3);%分别输出电机的力矩，当前电压，电流
            obj.current = con_value * obj.Kg;
            obj.voltage = obj.Ke*sj_obj.state(:,2) + obj.current*obj.Ra;%电压不太用得上，所以暂时不管了
            torque_motor_info(:,1) = obj.Ka * obj.current;
            torque_motor_info(:,2) = obj.voltage;
            torque_motor_info(:,3) = obj.current;     
        end
        
        function motor_info = simu_loop(obj,sj_obj,simu_period,con_value)        
            if(obj.working_mode == 0)
                motor_info = obj.vel_simu_loop(sj_obj,simu_period,con_value);
            else
                motor_info = obj.torque_simu_loop(sj_obj,con_value);
            end
        end
    end
    
   
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
end
